#include "manifold_cut_processor.h"

manifold_cut_processor::manifold_cut_processor(object *obj0, object *obj1)
{
	obj0_ = obj0;
	obj1_ = obj1;
	
	//create KDTree/ Tree Interval/ Space partioning etc, to speed up finding first triangle pair that intersects
	GetFirstTrianglePairThatIntersects();
	ManifoldCut();

	// Print any errors in any triangles formed.
	triangle tri;
	for(int i=0; i<obj0->GetTriangleSize(); i++)
	{
		tri = obj0->GetTriangleWithIndex(i);
		for(int j=0; j<3; j++)
			if(tri.version_to_tri[j] == -1)
				cout<<"triangle "<<tri.index<<" has version "<<j<<" = -1"<<endl;
	}
}
void manifold_cut_processor::GetFirstTrianglePairThatIntersects()
{
	int size0=obj0_->GetTriangleSize();
	int size1=obj1_->GetTriangleSize();
	
	// **Inefficient method: loop through all. 
	int index0, index1;
	Point pt0[3][3], pt1[3][3];
	start_tri_index1_ = start_tri_index0_ = -1;
	for(index0=0; index0<size0; index0++) {
		obj0_->GetAllVertexPtArrayWithTriangleIndex(index0, pt0);
		for(index1=0; index1<size1; index1++) {
			// Check if two triangles of index1 and index2 intersect
			obj1_->GetAllVertexPtArrayWithTriangleIndex(index1, pt1);
			if(moller.tri_tri_intersect(pt0[0],pt0[1],pt0[2],pt1[0],pt1[1],pt1[2])) {
				start_tri_index1_ = index1;
				start_tri_index0_ = index0;
				index1 = size1; 
				index0 = size0;
			}
		}
	}
	
	
	/*start_tri_index0_ = 3226; 
	start_tri_index1_ = 10;*/
	
	/*start_tri_index0_ = 6345;
	start_tri_index1_ = 1262;*/
	cout<<"First two intersection triangles = "<<start_tri_index0_<<" and "<<start_tri_index1_<<endl;
}
void manifold_cut_processor::ManifoldCut()
{
	Point pt1[3][3], pt0[3][3], intersectpt0[3], intersectpt1[3], traversal_end_pt[3];
	bool intersect = false;
	int *coplanar = new int;
	int type0, type1, type2, type3;
	int tri_index0 = start_tri_index0_;
	int tri_index1 = start_tri_index1_;

	while(true){
	//for(int i=0; i<3; i++) { cout<<"*** Note: Not going through all triangles cuz using fall loop."<<endl;
	
		// Get the triangles' points
		obj0_->GetAllVertexPtArrayWithTriangleIndex(tri_index0, pt0);
		obj1_->GetAllVertexPtArrayWithTriangleIndex(tri_index1, pt1);

		// Get intersection points
		intersect = moller.tri_tri_intersect_with_isectline(pt0[0], pt0[1], pt0[2], pt1[0], pt1[1], pt1[2], coplanar, intersectpt0, intersectpt1);
		
		if(obj0_->GetTriangleWithIndex(tri_index0).alive) {
			// Triangulate obj0_ 
			type0 = DeterminePointType(pt0, intersectpt0);
			type1 = DeterminePointType(pt0, intersectpt1);
			TriangleDivisionOnAdjacentTriangle(obj0_, tri_index0, pt0, intersectpt0, intersectpt1, type0, type1);
			TriangleDivisionOnTriangleThatIsAlive(obj0_,tri_index0, intersectpt0, intersectpt1, type0, type1);

			// Triangulate obj1_
			type2 = DeterminePointType(pt1, intersectpt0);
			type3 = DeterminePointType(pt1, intersectpt1);
			TriangleDivisionOnAdjacentTriangle(obj1_, tri_index1, pt1, intersectpt0, intersectpt1, type2, type3);
			TriangleDivisionOnTriangleThatIsAlive(obj1_,tri_index1, intersectpt0, intersectpt1, type2, type3);

			// Save intersection points
			if( rough_cut_path_.empty()) {	// random save
				rough_cut_path_.push_back( obj0_->GetVertexIndexWithPtArray(intersectpt0));
				rough_cut_path_.push_back( obj0_->GetVertexIndexWithPtArray(intersectpt1));
				exact_cut_path_.push_back( obj1_->GetVertexIndexWithPtArray(intersectpt0));
				exact_cut_path_.push_back( obj1_->GetVertexIndexWithPtArray(intersectpt1));
			}
			else {
				// for obj0
				if( obj0_->GetVertexIndexWithPtArray(intersectpt0) == rough_cut_path_.back())
					rough_cut_path_.push_back( obj0_->GetVertexIndexWithPtArray(intersectpt1));
				else
					rough_cut_path_.push_back( obj0_->GetVertexIndexWithPtArray(intersectpt0));

				// for obj1
				if( obj1_->GetVertexIndexWithPtArray(intersectpt0) == exact_cut_path_.back())
					exact_cut_path_.push_back( obj1_->GetVertexIndexWithPtArray(intersectpt1));
				else
					exact_cut_path_.push_back( obj1_->GetVertexIndexWithPtArray(intersectpt0));
			}
		}
		else {
			TriangleDivisionAtChildLevel(obj0_,obj1_, tri_index0, tri_index1, pt0, intersectpt0, intersectpt1, type0, type1);
		}

		// Get next two triangles
		GetNextTwoTrianglesThatIntersect(obj0_, obj1_, tri_index0, tri_index1, traversal_end_pt, intersectpt0, intersectpt1, type0, type1);
		cout<<"Next two triangles that intersect are: "<<tri_index0<<" and "<<tri_index1<<endl;
		
		if( obj0_->GetVertexIndexWithPtArray(traversal_end_pt) == rough_cut_path_.front())
			break;
	}
}
void manifold_cut_processor::TriangleDivisionAtChildLevel(object *obj0, object *obj1, int tri_index0, int tri_index1, Point parent_pt[3][3], Point intersectpt0[3], Point intersectpt1[3],int &intersect_type0, int &intersect_type1)
{
	Point starting_pt[3], ending_pt[3];
	Point result0[3], result1[3], result2[3], result3[3], result_end_pt[3], result_end_pt1[3];
	Point pt_array[3][3], pt_array1[3][3];
	int start_vertex_ref, vertex_ref0, vertex_ref1;
	int child_tri_index, child_tri_index1;
	int type0, type1, type2, type3;
	int result_end_pt_type, result_end_pt_type1;
	vector<int> neighbour_tri, neighbour_tri1;

	// Set type = -1 as default.
	intersect_type0 = intersect_type1 = -1;

	vertex_ref0 = obj0->GetVertexIndexWithPtArray(intersectpt0);
	vertex_ref1 = obj0->GetVertexIndexWithPtArray(intersectpt1);

	// Determine which intersect points is the starting and ending point
	// obj1 will follow obj0 so don't have to find for obj1.
	if( vertex_ref0 == rough_cut_path_.back()){
		op.SetPointEqualToPoint(intersectpt0, starting_pt);
		op.SetPointEqualToPoint(intersectpt1, ending_pt);
		intersect_type0 = 4;
	}
	else if( vertex_ref1 == rough_cut_path_.back()){
		op.SetPointEqualToPoint(intersectpt1, starting_pt);
		op.SetPointEqualToPoint(intersectpt0, ending_pt);
		intersect_type1 = 4;
	}
	else cout<<"Error in setting point in TriangleDivisionAtChildLevel"<<endl;
	
	// Find one child triangle that has the starting vertex ref, which is last element of the cut path vector
	child_tri_index = GetAliveChildTriangleIndexThatHasVertexIndex(obj0, tri_index0, rough_cut_path_.back());
	child_tri_index1 = GetAliveChildTriangleIndexThatHasVertexIndex(obj1, tri_index1, exact_cut_path_.back());
	
	while(true) {
		// If the first child tri intersects with the intersection pts, proceed to triangulation till end.
		// Else get the neighbour triangles and test for intersection.
		if(!GetTriangleIntersectionsWithCutSegment(obj0, child_tri_index, intersectpt0, intersectpt1, result0, result1)){
			GetNeighbourTrianglesOfTriangleIndexThatHasTheSameVertexIndex(obj0, child_tri_index, rough_cut_path_.back(), neighbour_tri);

			for(int i=0; i<neighbour_tri.size(); i++) {
				if(GetTriangleIntersectionsWithCutSegment(obj0, neighbour_tri[i], intersectpt0, intersectpt1, result0, result1)){
					child_tri_index = neighbour_tri[i];
					neighbour_tri.clear();
					break;
				}
			}
		}
		// Use the cutting result segment on obj1
		if(!GetTriangleIntersectionsWithCutSegment(obj1, child_tri_index1, result0, result1, result2, result3)){
			GetNeighbourTrianglesOfTriangleIndexThatHasTheSameVertexIndex(obj1, child_tri_index1, exact_cut_path_.back(), neighbour_tri1);

			for(int i=0; i<neighbour_tri1.size(); i++) {
				if(GetTriangleIntersectionsWithCutSegment(obj1, neighbour_tri1[i], result0, result1, result2, result3)){
					child_tri_index1 = neighbour_tri1[i];
					neighbour_tri1.clear();
					break;
				}
			}
		}

		// Triangulate obj0_
		obj0->GetAllVertexPtArrayWithTriangleIndex(child_tri_index, pt_array);
		type0 = DeterminePointType(pt_array, result0);
		type1 = DeterminePointType(pt_array, result1);
		TriangleDivisionOnAdjacentTriangle(obj0, child_tri_index, pt_array, result0, result1, type0, type1);
		TriangleDivisionOnTriangleThatIsAlive(obj0, child_tri_index, result0, result1, type0, type1);
	
		// Triangulate obj1_
		// Set the cutting result of obj1 as the cutting result of obj0 because they should be the same.
		op.SetPointEqualToPoint(result0, result2);
		op.SetPointEqualToPoint(result1, result3);
		obj1->GetAllVertexPtArrayWithTriangleIndex(child_tri_index1, pt_array1);
		type2 = DeterminePointType(pt_array1, result2);
		type3 = DeterminePointType(pt_array1, result3);
		TriangleDivisionOnAdjacentTriangle(obj1_, child_tri_index1, pt_array1, result2, result3, type2, type3);
		TriangleDivisionOnTriangleThatIsAlive(obj1_,child_tri_index1, result2, result3, type2, type3);


		// Determine which result point is the end point of the cut
		if( obj0->GetVertexIndexWithPtArray(result0) == rough_cut_path_.back()) {
			op.SetPointEqualToPoint(result1, result_end_pt);
			result_end_pt_type = type1;
		}
		else {
			op.SetPointEqualToPoint(result0, result_end_pt);
			result_end_pt_type = type0;
		}

		if( obj1->GetVertexIndexWithPtArray(result2) == exact_cut_path_.back()) {
			op.SetPointEqualToPoint(result3, result_end_pt1);
			result_end_pt_type1 = type3;
		}
		else {
			op.SetPointEqualToPoint(result2, result_end_pt1);
			result_end_pt_type1 = type2;
		}

		// Save the cutting end points to the cut path
		rough_cut_path_.push_back( obj0->GetVertexIndexWithPtArray(result_end_pt));
		exact_cut_path_.push_back( obj1->GetVertexIndexWithPtArray(result_end_pt1));

		// Check whether we should stop cutting.
		if( op.AreTwoPointsEqual(result_end_pt, ending_pt)){
			// stop cutting and determine the intersection types, which will be used in ManifoldCut()
			if( intersect_type0 == -1)
				intersect_type0 = DeterminePointType(parent_pt, result_end_pt);
			else
				intersect_type1 = DeterminePointType(parent_pt, result_end_pt);
			return;
		}
		
		// Traverse to adjacent triangle, which are child of root triangle, and cut
		child_tri_index = obj0->GetTriangleWithIndex(child_tri_index).version_to_tri[result_end_pt_type];

		if(result_end_pt_type1 <3 ) // possible that type=3. Type=4 will be resolved at the top where it finds the neighbours.
			child_tri_index1 = obj1->GetTriangleWithIndex(child_tri_index1).version_to_tri[result_end_pt_type1];

		// Get the sub child triangle that intersects with the intersection points
		child_tri_index = GetAliveChildTriangleIndexThatHasVertexIndex(obj0, child_tri_index, rough_cut_path_.back());
		child_tri_index1 = GetAliveChildTriangleIndexThatHasVertexIndex(obj1, child_tri_index1, exact_cut_path_.back());
	}
}
int manifold_cut_processor::GetAliveChildTriangleIndexThatHasVertexIndex(object *obj, int parent_index, int vertex_index)
{
	triangle tri = obj->GetTriangleWithIndex(parent_index);
	triangle child_tri;
	int result_from_sub_child;

	if( tri.alive) {
		if(obj->TriangleIndexHasVertexIndex(tri.index, vertex_index))
			return tri.index;
		else 
			return -1;
	}
	for(int i=0; i<tri.child_tri.size(); i++) {
		child_tri = obj->GetTriangleWithIndex(tri.child_tri[i]);
		if(child_tri.alive) {
			if(obj->TriangleIndexHasVertexIndex(child_tri.index, vertex_index))
				return child_tri.index;
		}
		else {
			result_from_sub_child = GetAliveChildTriangleIndexThatHasVertexIndex(obj, child_tri.index, vertex_index);
			if( result_from_sub_child != -1)
				return result_from_sub_child;
		}
	}
	return -1;
}
// Function returns the a value that tells where the point 'intersectpt' lies in the triangle of 'tri_pt'
// 0 = edge0; 1 = edge1; 2 = edge2; 3 = within triangle; 4 = vertex of triangle; -1 = outside triangle;
int manifold_cut_processor::DeterminePointType(Point tri_pt[3][3], Point intersectpt[3])
{
	for(int j=0; j<3; j++)
		if(op.AreTwoPointsEqual(tri_pt[j], intersectpt))
			return 4;
	for(int i=0; i<3; i++)
		if(op.PointIsWithinTwoPoints(intersectpt, tri_pt[i], tri_pt[(i+1)%3]))
			return i;
	if(op.PointIsInsideTriangle(tri_pt, intersectpt))
		return 3;

	return -1;
}

void manifold_cut_processor::TriangleDivisionOnAdjacentTriangle(object *obj, int tri_index, Point pt[3][3],Point r0[3], Point r1[3], int type0, int type1)
{
	if( (type0==4 && (type1>=0||type1<=2) )|| (type1==4 && (type0>=0||type0<=2) ))
		if(op.BothPointsOnTheSameEdge(r0, r1, pt)) {
			cout<<"No triangle division at adj triangle because both points are on the same edge"<<endl;
			return;
		}
	triangle current_tri = obj->GetTriangleWithIndex(tri_index);
	if(type0 >=0 && type0 <3) 
		EdgePointDivisionOnAdjacentTriangle(obj, current_tri, r0, type0);
	if(type1 >=0 && type1 <3)
		EdgePointDivisionOnAdjacentTriangle(obj, current_tri, r1, type1);
}
void manifold_cut_processor::EdgePointDivisionOnAdjacentTriangle(object *obj, triangle current_tri,  float r[3], int type)
{
	triangle adj_tri = obj->GetTriangleWithIndex(current_tri.version_to_tri[type] );
	int adj_tri_edge_type = obj->GetEdgeOfTriangle(adj_tri, current_tri.pt[type],current_tri.pt[(type+1)%3]);

	if( adj_tri.alive)
		EdgePointDivision(obj, adj_tri.index, r, adj_tri_edge_type);
	else {
		int adj_child_index = obj->FindChildTriThatHasPointInEdge(adj_tri, r, adj_tri_edge_type);
		if(adj_tri_edge_type != 4)
			EdgePointDivision(obj, adj_child_index, r, adj_tri_edge_type);
	}
}
void manifold_cut_processor::TriangleDivisionOnTriangleThatIsAlive(object *obj,int tri_index, Point r0[3], Point r1[3], int type0, int type1)
{
	Point dummy0[3], dummy1[3];
	int dummytype0, dummytype1;
	triangle parent_tri;

	if(type0>=0 && type0<3)
		EdgePointDivision(obj, tri_index, r0, type0);
	if(type0 == 3)
		EndPointDivision(obj, tri_index, r0);

	// type0 == 4
	if(obj->GetTriangleWithIndex(tri_index).alive) {	
		if(type1>=0 && type1<3)
			EdgePointDivision(obj, tri_index, r1, type1);
		if(type1 == 3)
			EndPointDivision(obj, tri_index, r1);
	}
	else if(type1 != 4) {
		// Find which child intersects with the cut segment r0 - r1;
		parent_tri = obj->GetTriangleWithIndex(tri_index);
		for( int i=0; i < parent_tri.child_tri.size(); i++) {
			if(GetTriangleIntersectionsWithCutSegment(obj, parent_tri.child_tri[i],r0, r1, dummy0, dummy1)){
				if(type1 == 3)
					EndPointDivision(obj, parent_tri.child_tri[i], r1);
				if(type1>=0 && type1<3) {
					// Get the edge of child tri that shares with the edge of parent tri (using type1)
					int new_type = obj->GetEdgeOfTriangleThatHasVertexIndexAsStartingPointOfEdge(obj->GetTriangleWithIndex(parent_tri.child_tri[i]), parent_tri.pt[type1]);
					EdgePointDivision(obj, parent_tri.child_tri[i], r1, new_type);
				}
				return;
			}
		}
	}

}
bool manifold_cut_processor::GetTriangleIntersectionsWithCutSegment(object *obj, int tri_index, Point cut_pt0[3], Point cut_pt1[3], Point r0[3], Point r1[3])
{
	Point tri_pt[3][3], intersectpt[3][3];
	obj->GetAllVertexPtArrayWithTriangleIndex(tri_index, tri_pt);
	queue<Point*> storage;

	for(int i=0; i<3; i++) {
		if(op.FindSegmentEdgeIntersection(cut_pt0, cut_pt1, tri_pt[i], tri_pt[(i+1)%3], intersectpt[i])) {
			if( storage.empty())
				storage.push(intersectpt[i]);
			else {
				if( !op.AreTwoPointsEqual(intersectpt[i], storage.front()) && 
					!op.AreTwoPointsEqual(intersectpt[i], storage.back()))
					storage.push(intersectpt[i]); // do no store duplicates
			}
		}
	}
	if(storage.empty())
		return false;
	if(storage.size() == 2) {
		op.SetPointEqualToPoint(storage.front(), r0);
		op.SetPointEqualToPoint(storage.back(), r1);
	}
	else {	// one of the cut points may be inside the triangle and not the intersection point.
		op.SetPointEqualToPoint(storage.front(), r0);
		if( op.PointIsInsideTriangle(tri_pt, cut_pt0) && !op.AreTwoPointsEqual(r0, cut_pt0))
			op.SetPointEqualToPoint(cut_pt0, r1);
		else 
			if( op.PointIsInsideTriangle(tri_pt, cut_pt1) && !op.AreTwoPointsEqual(r0, cut_pt1))
				op.SetPointEqualToPoint(cut_pt1, r1);
			else
				return false;
	}
	return true;
}
void manifold_cut_processor::EdgePointDivision(object *obj, int parent_index, float p[3], int edge)
{
	cout<<"/********EdgePointDivision*************/"<<endl;
	triangle parentTriangle = obj->GetTriangleWithIndex(parent_index);
	triangle adjcentTriangle;
	int vertices[3], version_to_tri[3], p_index;

	// edge point division will creat 2 smaller triangles
	// given their indices as index0, index0+1,
	int index0 = obj->GetTriangleSize();
	p_index = obj->AddVertex(p);

	// triangle 1
	vertices[0] = parentTriangle.pt[edge];
	vertices[1] = p_index;
	vertices[2] = parentTriangle.pt[(edge+2)%3];

	// Version 0 will always be unknown so it's either in unknown_shared_edge_vector 
	// or -1 for the time being until the other adj tri is created.
	version_to_tri[0] = CompareEdgeAndReturnTriangleIndex(obj, vertices[0], vertices[1], index0, 0); // adjcent triangle handled automatically
	version_to_tri[1] = index0+1;
	version_to_tri[2] = parentTriangle.version_to_tri[(edge+2)%3];
	
	obj->AddTriangle(vertices, version_to_tri);
	obj->AddChildTriangle(parent_index, index0);

	// triangle 2
	vertices[0] = p_index;
	vertices[1] = parentTriangle.pt[(edge+1)%3];
	vertices[2] = parentTriangle.pt[(edge+2)%3];
	
	version_to_tri[0] = CompareEdgeAndReturnTriangleIndex(obj, vertices[0], vertices[1], index0+1, 0);
	version_to_tri[1] = parentTriangle.version_to_tri[(edge+1)%3];
	version_to_tri[2] = index0;

	obj->AddTriangle(vertices, version_to_tri);

	obj->AddChildTriangle(parent_index, index0+1);
	obj->SetTriangleAsDeadWithIndex(parent_index);
	cout<<"/*************End*************/"<<endl;
}
void manifold_cut_processor::EndPointDivision(object *obj, int parent_index, float p[3])
{
	cout<<"/*************EndPointDivision*************/"<<endl;
	triangle parentTriangle = obj->GetTriangleWithIndex(parent_index);
	triangle adjacentTriangle;

	// each end point division will creat 3 smaller triangles
	// given their indices as index0, index0+1, index0+2
	int index0 = obj->GetTriangleSize();
	int p_index = obj->AddVertex(p);

	int vertices[3], version_to_tri[3];

	for ( int i=0; i<3; i++)
	{
		vertices[0] = parentTriangle.pt[i]; 
		vertices[1] = parentTriangle.pt[(i+1)%3];
		vertices[2] = p_index;

		version_to_tri[0] = parentTriangle.version_to_tri[i];
		version_to_tri[1] = index0+(i+1)%3;
		version_to_tri[2] = index0+(i+2)%3;

		obj->AddTriangle(vertices, version_to_tri);

		// Update adj triangle 
		if ( parentTriangle.version_to_tri[i] != -1 ) {
			// Get adj triangle 
			adjacentTriangle = obj->GetTriangleWithIndex(parentTriangle.version_to_tri[i]);
			// Compute version of adj tri where new tri is at
			int version = obj->GetEdgeOfTriangle(adjacentTriangle, parentTriangle.pt[i], parentTriangle.pt[(i+1)%3]);
			// Update
			obj->UpdateTriangleVersionToTri(adjacentTriangle.index, version, index0+i);
		}
		obj->AddChildTriangle(parent_index, index0+i);
	}
	obj->SetTriangleAsDeadWithIndex(parent_index);
	cout<<"/*************End*************/"<<endl;
}
int manifold_cut_processor::CompareEdgeAndReturnTriangleIndex(object *obj, int p0, int p1, int tri_index, int version_index)
{
	if( unknown_shared_edge.empty()) {
		cout<<"Unknown_shared_edge is empty."<<endl;
		AddUnknownSharedEdge(p0, p1, tri_index, version_index);
		return -1;
	}

	vector<edge>::iterator iter = unknown_shared_edge.begin();
	while( iter != unknown_shared_edge.end() ) {
		if( (iter->pt0 == p0 && iter->pt1 == p1) || (iter->pt0 == p1 && iter->pt1 == p0)) {
			int returnIndex = iter->tri_index;
			obj->UpdateTriangleVersionToTri(returnIndex, iter->version, tri_index);
			unknown_shared_edge.erase(iter);
			return returnIndex;
		}
		iter++;
	}

	// no edge found
	AddUnknownSharedEdge(p0, p1, tri_index, version_index);
	return -1;
}
void manifold_cut_processor::AddUnknownSharedEdge(int p0, int p1, int tri_index, int version)
{
	edge unknown_edge;
	unknown_edge.pt0 = p0;
	unknown_edge.pt1 = p1;
	unknown_edge.tri_index = tri_index;
	unknown_edge.version = version;
	unknown_shared_edge.push_back(unknown_edge);
}
void manifold_cut_processor::GetNextTwoTrianglesThatIntersect(object *obj0, object *obj1, int &tri_index0, int &tri_index1,Point traversal_end_pt[3], Point intersectpt0[3], Point intersectpt1[3], int type0, int type1)
{
	Point tri_1_pt[3][3];
	int obj0_type, obj1_type;

	GetTraversalEndPoint(obj0, intersectpt0, intersectpt1, traversal_end_pt);
	cout<<"Traversal end point is vertex index = "<<obj0->GetVertexIndexWithPtArray(traversal_end_pt)<<endl;

	if(obj0->GetVertexIndexWithPtArray(traversal_end_pt) == rough_cut_path_.front())
		return;	// back to starting triangle so do nothing.

	// Determine point type of obj0. 
	if( op.AreTwoPointsEqual(traversal_end_pt, intersectpt0))
		obj0_type = type0;
	else if( op.AreTwoPointsEqual(traversal_end_pt, intersectpt1))
			obj0_type = type1;
	else cout<<"Error! In GetNextTwoTrianglesThatIntersect(), traversal_end_pt is initiated incorrectly or precision error in comparison."<<endl;

	// Determine point type of obj1
	obj1->GetAllVertexPtArrayWithTriangleIndex(tri_index1, tri_1_pt);
	obj1_type = DeterminePointType(tri_1_pt, traversal_end_pt);
	
	cout<<"obj0type="<<obj0_type<<" and obj1type="<<obj1_type<<endl;
	if(obj0_type == 4 && obj1_type == 4) {
		MultipleTriangleWithMultipleTriangle(obj0, obj1, tri_index0, tri_index1, traversal_end_pt);
	}
	if(obj0_type == 4 && obj1_type != 4) {
		MultipleTriangleWithTheSameOrAdjacentTriangle(obj1, obj0, tri_index1, tri_index0, obj1_type, obj0_type, traversal_end_pt);
	}
	if(obj0_type != 4 && obj1_type == 4) {
		MultipleTriangleWithTheSameOrAdjacentTriangle(obj0, obj1, tri_index0, tri_index1, obj0_type, obj1_type, traversal_end_pt);
	}
	if(obj0_type != 4 && obj1_type != 4) {
		ContinueUsingTheSameTriangleOrAdjacentTriangle(obj0, obj1, tri_index0, tri_index1, obj0_type, obj1_type);
	}
}
void manifold_cut_processor::GetTraversalEndPoint(object *obj, Point intersectpt0[3], Point intersectpt1[3], Point traversal_end_pt[3])
{
	if( rough_cut_path_.back() == obj->GetVertexIndexWithPtArray(intersectpt0)){
		op.SetPointEqualToPoint(intersectpt0, traversal_end_pt);
	}
	else{ 
		op.SetPointEqualToPoint(intersectpt1, traversal_end_pt);
	}
}
void manifold_cut_processor::ContinueUsingTheSameTriangleOrAdjacentTriangle(object *obj0, object *obj1, int &tri_index0, int &tri_index1, int type0, int type1)
{
	if( type0 == 3 && type1 != 3){
		tri_index1 = obj1->GetTriangleWithIndex(tri_index1).version_to_tri[type1];
		return;	// tri_index0 remains
	}
	if( type0 != 3 && type1 == 3){
		tri_index0 = obj0->GetTriangleWithIndex(tri_index0).version_to_tri[type0];
		return; // tri_index1 remains
	}

	int adj_tri_index0 = obj0->GetTriangleWithIndex(tri_index0).version_to_tri[type0];
	int adj_tri_index1 = obj1->GetTriangleWithIndex(tri_index1).version_to_tri[type1];

	Point pt0[3][3], pt1[3][3], adj_pt0[3][3], adj_pt1[3][3];
	obj0->GetAllVertexPtArrayWithTriangleIndex(tri_index0, pt0);
	obj0->GetAllVertexPtArrayWithTriangleIndex(adj_tri_index0, adj_pt0);
	obj1->GetAllVertexPtArrayWithTriangleIndex(tri_index1, pt1);
	obj1->GetAllVertexPtArrayWithTriangleIndex(adj_tri_index1, adj_pt1);

	if( moller.tri_tri_intersect(adj_pt0[0],adj_pt0[1],adj_pt0[2], adj_pt1[0], adj_pt1[1], adj_pt1[2])){
		tri_index0 = adj_tri_index0;
		tri_index1 = adj_tri_index1;
		return;
	}
	if( moller.tri_tri_intersect(adj_pt0[0],adj_pt0[1],adj_pt0[2], pt1[0], pt1[1], pt1[2])){
		tri_index0 = adj_tri_index0;
		tri_index1 = tri_index1;
		return;
	}
	if( moller.tri_tri_intersect(pt0[0],pt0[1],pt0[2], adj_pt1[0], adj_pt1[1], adj_pt1[2])){
		tri_index0 = tri_index0;
		tri_index1 = adj_tri_index1;
		return;
	}
	cout<<"Error! ContinueUsingSameTriangle() doesn't get the correct next two triangles to intersect."<<endl;
}
void  manifold_cut_processor::MultipleTriangleWithMultipleTriangle(object *obj0, object *obj1, int &tri_index0, int &tri_index1, Point end_pt[3])
{
	vector<int> tri_neighbour0, tri_neighbour1;
	int vtx_ref0, vtx_ref1;
	Point p[3][3], q[3][3];

	// Get end point's vertex index reference
	vtx_ref0 = obj0->GetVertexIndexWithPtArray(end_pt);
	vtx_ref1 = obj1->GetVertexIndexWithPtArray(end_pt);

	// Get all the triangles around the end_pt to test for intersection.
	tri_neighbour0.push_back(tri_index0);	// also test the first tri_index0 with neighbours of tri_index1
	GetNeighbourTrianglesOfTriangleIndexThatHasTheSameVertexIndex(obj0, tri_index0, vtx_ref0, tri_neighbour0);
	GetNeighbourTrianglesOfTriangleIndexThatHasTheSameVertexIndex(obj1, tri_index1, vtx_ref1, tri_neighbour1);

	for(int i=0; i<tri_neighbour0.size(); i++) {
		obj0->GetAllVertexPtArrayWithTriangleIndex(tri_neighbour0[i], p);

		for(int j=0; j<tri_neighbour1.size(); j++) {
			obj1->GetAllVertexPtArrayWithTriangleIndex(tri_neighbour1[j], q);
			if(moller.tri_tri_intersect(p[0],p[1],p[2], q[0],q[1],q[2])){
				tri_index0 = tri_neighbour0[i];
				tri_index1 = tri_neighbour1[j];
				return;
			}
		}
	}
	cout<<"Error! MultipleTriangleWithMultipleTriangle() can't find two triangles that intersect."<<endl;
}
void manifold_cut_processor::MultipleTriangleWithTheSameOrAdjacentTriangle(object *fix_adj_obj, object *multi_obj, int &fix_adj_tri_index, int &multi_tri_index, int fix_adj_type, int multi_type, Point end_pt[3])
{
	Point fix_pt[3][3], multi_pt[3][3];
	vector<int> neighbour_tri;
	int fix_tri_index, vtx_ref;

	// Get the fix/adj triangle's adjacent triangle to test with multiply triangles for intersection
	if( fix_adj_type>=0 && fix_adj_type<3) 
		fix_tri_index = fix_adj_obj->GetTriangleWithIndex(fix_adj_tri_index).version_to_tri[fix_adj_type];
	else 
		fix_tri_index = fix_adj_tri_index;
	
	fix_adj_obj->GetAllVertexPtArrayWithTriangleIndex(fix_tri_index, fix_pt);

	// Get end point's vertex index reference
	vtx_ref = multi_obj->GetVertexIndexWithPtArray(end_pt);

	// Get all the triangles around the end_pt to test for intersection.
	GetNeighbourTrianglesOfTriangleIndexThatHasTheSameVertexIndex(multi_obj, multi_tri_index, vtx_ref, neighbour_tri);

	for(int i=0; i<neighbour_tri.size(); i++) {
		multi_obj->GetAllVertexPtArrayWithTriangleIndex(neighbour_tri[i], multi_pt);

		if( moller.tri_tri_intersect(fix_pt[0],fix_pt[1],fix_pt[2], multi_pt[0],multi_pt[1],multi_pt[2])){
			multi_tri_index = neighbour_tri[i];
			fix_adj_tri_index = fix_tri_index;
			return;
		}
	}
	// Get the original triangle to test with multiple triangles
	if( fix_adj_type != 3) 
	{
		fix_tri_index = fix_adj_tri_index;
		fix_adj_obj->GetAllVertexPtArrayWithTriangleIndex(fix_adj_tri_index, fix_pt);

		for(int i=0; i<neighbour_tri.size(); i++) {
			multi_obj->GetAllVertexPtArrayWithTriangleIndex(neighbour_tri[i], multi_pt);

			if(moller.tri_tri_intersect(fix_pt[0],fix_pt[1],fix_pt[2], multi_pt[0],multi_pt[1],multi_pt[2])){
				multi_tri_index = neighbour_tri[i];
				fix_adj_tri_index = fix_tri_index;
				return;
			}
		}
	}
	cout<<"Error! MultipleTriangleWithTheSameOrAdjacentTriangle() unable to find two triangles that intersect."<<endl;
}
void manifold_cut_processor::GetNeighbourTrianglesOfTriangleIndexThatHasTheSameVertexIndex(object *obj, int tri_index, int vertex_ref, vector<int> &neighbour_tri)
{
	int adj_tri_index;
	triangle tri = obj->GetTriangleWithIndex(tri_index);
	adj_tri_index = tri.version_to_tri[obj->GetEdgeOfTriangleThatHasVertexIndexAsStartingPointOfEdge(tri ,vertex_ref)];
	do{
		neighbour_tri.push_back(adj_tri_index);
		tri = obj->GetTriangleWithIndex(adj_tri_index);
		adj_tri_index = tri.version_to_tri[obj->GetEdgeOfTriangleThatHasVertexIndexAsStartingPointOfEdge(tri ,vertex_ref)];
	}while( adj_tri_index != tri_index);
}